/*
 * File      : application.c
 * This file is part of RT-Thread RTOS
 * COPYRIGHT (C) 2014, RT-Thread Development Team
 *
 * The license and distribution terms for this file may be
 * found in the file LICENSE in this distribution or at
 * http://www.rt-thread.org/license/LICENSE
 *
 * Change Logs:
 * Date           Author       Notes
 * 2016-12-05     Pluto        first implementation
 */
/**
 * @addtogroup NUVOTON_M451
 */

/*@{*/

#include <stdio.h>

#include <board.h>
#include <rtthread.h>

#include "led.h"

#include "TARM_Hardware/tarm_io.h"
#include "TARM_Hardware/tarm_basetimer.h"

#include "TARM_Application/tarm_init.h"
#include "TARM_Application/tarm_run.h"


/* led thread entry */
static void led_thread_entry(void *parameter)
{
    rt_hw_led_init();
    //rt_kprintf("start\n");
    while (1)
    {
        rt_hw_led_on();
        rt_thread_delay(RT_TICK_PER_SECOND/2);
//			TIMER_Delay(TIMER0, 1000);
        rt_hw_led_off();
        rt_thread_delay(RT_TICK_PER_SECOND/2);
//			TIMER_Delay(TIMER0, 1000);
        // rt_thread_resume(printf_thread);
    }
}

static void TARM_DriverStepSteering(void *parameter)
{
    char t_strReport[30];
    set_current_start();
    while (1)
    {
        if (g_TARM_RunRecord.StartRun_pv == 0 && g_TARM_RunRecord.StartRun_r == 0)
        {
            //让出CPU
            rt_thread_mdelay(500);
            // rt_thread_suspend(rt_thread_self());
            // rt_schedule();
        }
        else if (g_TARM_RunRecord.StartRun_pv != 0)
        {
            //驱动
            handle_pvMainLoop(g_TARM_RunRecord.ReadyRunLoc_x,
                              g_TARM_RunRecord.ReadyRunLoc_y,
                              g_TARM_RunRecord.ReadyRunLoc_speed);
            g_TARM_RunRecord.StartRun_pv = 0;
            sprintf(t_strReport, "->pv arrive:\nx:%f\ty:%f\n",
                    g_TARM_RunRecord.pv_current.x,
                    g_TARM_RunRecord.pv_current.y);
            rt_kprintf(t_strReport);
        }
        else if (g_TARM_RunRecord.StartRun_r != 0)
        {
            handle_rMainLoop(g_TARM_RunRecord.ReadyRunLoc_r,
                             g_TARM_RunRecord.ReadyRunLoc_speedr);
            g_TARM_RunRecord.StartRun_r = 0;
            rt_kprintf("->r arrive");
        }
    }
}

// led thread entry
static void printf_period(void *parameter)
{
    rt_base_t t_thread_int_onoff;
	char t_strReport[30];
    rt_kprintf("heart break start\n");
    rt_thread_mdelay(1000);
    while (1)
    {
//        rt_kprintf("heart break1\n");
//        //        
//        // rt_thread_suspend(rt_thread_self());
//        // rt_schedule();
//			
//        rt_thread_mdelay(1000);
//        t_thread_int_onoff = rt_hw_interrupt_disable();
//        rt_kprintf("heart break2\n");
//        TIM0_Delay(3000);
//        while (TIMER0->INTSTS & TIMER_INTSTS_TIF_Msk)
//          ;
//        rt_hw_interrupt_enable(t_thread_int_onoff);
		g_TARM_RunRecord.ReadyRunLoc_x = 10;
		g_TARM_RunRecord.ReadyRunLoc_y = 14;
		g_TARM_RunRecord.ReadyRunLoc_speed = 120;
		g_TARM_RunRecord.StartRun_pv =1;
		rt_kprintf("_&x:10;\t_&y:14;\n_&speed:150;\r\r\n");
		while(g_TARM_RunRecord.StartRun_pv == 1){
			rt_thread_mdelay(1000);
		}
		g_TARM_RunRecord.ReadyRunLoc_x = 17;
		g_TARM_RunRecord.ReadyRunLoc_y = 5;
		g_TARM_RunRecord.ReadyRunLoc_speed = 120;
		g_TARM_RunRecord.StartRun_pv =1;
		rt_kprintf("_&x:14;\t_&y:5;\n_&speed:150;\r\r\n");
		while(g_TARM_RunRecord.StartRun_pv == 1){
			rt_thread_mdelay(1000);
		}
		
//		g_TARM_RunRecord.ReadyRunLoc_r = 80;
//		g_TARM_RunRecord.ReadyRunLoc_speedr = 500;
//		g_TARM_RunRecord.StartRun_r =1;
//		rt_kprintf("_&r:80;\n_&speed:150;\r\r\n");
//		while(g_TARM_RunRecord.StartRun_r == 1){
//			rt_thread_mdelay(1000);
//		}
		g_TARM_RunRecord.ReadyRunLoc_x = 17;
		g_TARM_RunRecord.ReadyRunLoc_y = 0;
		g_TARM_RunRecord.ReadyRunLoc_speed = 120;
		g_TARM_RunRecord.StartRun_pv =1;
		rt_kprintf("_&x:14;\t_&y:5;\n_&speed:150;\r\r\n");
		while(g_TARM_RunRecord.StartRun_pv == 1){
			rt_thread_mdelay(1000);
		}
		
		g_TARM_RunRecord.ReadyRunLoc_x = 17;
		g_TARM_RunRecord.ReadyRunLoc_y = 17;
		g_TARM_RunRecord.ReadyRunLoc_speed = 120;
		g_TARM_RunRecord.StartRun_pv =1;
		rt_kprintf("_&x:14;\t_&y:5;\n_&speed:150;\r\r\n");
		while(g_TARM_RunRecord.StartRun_pv == 1){
			rt_thread_mdelay(1000);
		}
		
//		g_TARM_RunRecord.ReadyRunLoc_r = -80;
//		g_TARM_RunRecord.ReadyRunLoc_speedr = 500;
//		g_TARM_RunRecord.StartRun_r =1;
//		rt_kprintf("_&r:80;\n_&speed:150;\r\r\n");
//		while(g_TARM_RunRecord.StartRun_r == 1){
//			rt_thread_mdelay(1000);
//		}
    }
}

static void rt_init_thread_entry(void *parameter)
{
	rt_thread_t led_thread, printf_thread, tarm_drivessThread;

/* Initialization RT-Thread Components */
#ifdef RT_USING_COMPONENTS_INIT
    rt_components_init();
#endif

    /* Create led thread */
    led_thread = rt_thread_create("led",
                                  led_thread_entry, RT_NULL,
                                  256, 20, 20);
    if (led_thread != RT_NULL)
        rt_thread_startup(led_thread);

//    printf_thread = rt_thread_create("print period",
//                                     printf_period, RT_NULL,
//                                     512, 20, 20);
//    if (printf_thread != RT_NULL)
//        rt_thread_startup(printf_thread);

     tarm_drivessThread = rt_thread_create("TARM DriverSS",
                                      TARM_DriverStepSteering, RT_NULL,
                                      1024, 20, 300);
     if (tarm_drivessThread != RT_NULL)
         rt_thread_startup(tarm_drivessThread);
}

int rt_application_init()
{
    rt_thread_t init_thread;
		SteeringIO_Init();
		io_init();
//		locate_init();
	
#if (RT_THREAD_PRIORITY_MAX == 32)
    init_thread = rt_thread_create("init",
                                   rt_init_thread_entry, RT_NULL,
                                   512, 8, 20);
#else
    init_thread = rt_thread_create("init",
                                   rt_init_thread_entry, RT_NULL,
                                   512, 80, 20);
#endif
    if (init_thread != RT_NULL)
        rt_thread_startup(init_thread);

    return 0;
}

/*@}*/
